/**
 * Author(s): xuming.Wong <xuming.Wong@gmail.com>
 */
#include "DynamicsWorld.h"

#include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h"
#include "BulletCollision/BroadphaseCollision/btAxisSweep3.h"
#include "BulletCollision/BroadphaseCollision/btDbvtBroadphase.h"

#include "PhysicsConfig.h"
#include "SceneObject.h"


namespace PQEngine
{
	DynamicsWorld::DynamicsWorld()
		: _dynamicsWorld(0),_constraintSolver(0),_dispatcher(0),_collisionConfiguration(0),_broadphase(0)
	{
		
	}

	DynamicsWorld& DynamicsWorld::getSingleton()
	{
		static DynamicsWorld inst;
		return inst;
	}

	void DynamicsWorld::init()
	{
		initPhysics();
		initTerrain();
		initGameObject(SceneObject::getActive()->getRoot());
	}

	void DynamicsWorld::initPhysics()
	{
		BroadPhaseType type= PhysicsConfig::getSingleton().getBroadPhaseType();
		Ogre::Vector3 gravity=PhysicsConfig::getSingleton().getGravity();
		
		_collisionConfiguration=new btDefaultCollisionConfiguration();
		_dispatcher = new btCollisionDispatcher(_collisionConfiguration);
		
		switch(type){
			case BPT_DBVT:
				_broadphase=new btDbvtBroadphase();
				break;
			case BPT_AXISSWEEP3_32:
				{
					Ogre::AxisAlignedBox bounds=PhysicsConfig::getSingleton().getWorldBounds();
					btVector3 min=MathUtil::toBullet(bounds.getMinimum());
					btVector3 max=MathUtil::toBullet(bounds.getMaximum());
					_broadphase=new bt32BitAxisSweep3(min,max);
					break;
				}
			default:
				_broadphase=new btDbvtBroadphase();
				break;
		}

		_constraintSolver= new btSequentialImpulseConstraintSolver();
		_dynamicsWorld=new btDiscreteDynamicsWorld(_dispatcher, _broadphase, _constraintSolver,_collisionConfiguration);
		_dynamicsWorld->setGravity(btVector3(gravity.x,gravity.y,gravity.z));
	}

	void DynamicsWorld::initTerrain()
	{
		TerrainObject* object=TerrainObject::head();
		while(object){
			initTerrainObject(object);
			object=object->next();
		}
	}

	void DynamicsWorld::initTerrainObject(TerrainObject* object)
	{
		RigidTerrain* terrain=new RigidTerrain(object);
		if(terrain->getRigidBody()){
			_dynamicsWorld->addRigidBody(terrain->getRigidBody());
		}
	}

	void DynamicsWorld::initGameObject(GameObject *object)
	{
		if(object==NULL){
			return;
		}

		if(object->isRigidBody()){
			RigidGameObject* rigidBody=new RigidGameObject(object);
			addRigidBodyObject(rigidBody);
		}

		int size=object->getChildrenCount();
		for(int i=0;i<size;i++){
			GameObject* child=object->getChild(i);
			initGameObject(child);
		}
	}

    void DynamicsWorld::addRigidBodyObject(RigidGameObject* rigidObject)
    {
		//should save rigidbody ptr in list?
		if(rigidObject){
			if(rigidObject->getRigidBody()){
				_dynamicsWorld->addRigidBody(rigidObject->getRigidBody());
			}
		}
    }

    void DynamicsWorld::stepSimulation(const float elapsedTime)
    {		
		int maxSubstep=PhysicsConfig::getSingleton().getMaxSubStep();
		float fixedTimeStep=PhysicsConfig::getSingleton().getFixedTimeStep();
		_dynamicsWorld->stepSimulation(elapsedTime,maxSubstep,fixedTimeStep);
		
	}

	void DynamicsWorld::calcContacts()
	{
		int numManifolds=_dynamicsWorld->getDispatcher()->getNumManifolds();
		for(int i=0;i<numManifolds;i++){
			btPersistentManifold* contactManifold=_dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i);
			btCollisionObject* object0=static_cast<btCollisionObject*>(contactManifold->getBody0());
			btCollisionObject* object1=static_cast<btCollisionObject*>(contactManifold->getBody1());
			contactManifold->refreshContactPoints(object0->getWorldTransform(),object1->getWorldTransform());

			int numContacts=contactManifold->getNumContacts();
			for(int j=0;j<numContacts;j++){
				btManifoldPoint& pt=contactManifold->getContactPoint(j);
				btVector3 ptA=pt.getPositionWorldOnA();
				btVector3 ptB=pt.getPositionWorldOnB();
				btVector3 ptDistB=ptB+pt.m_normalWorldOnB*100;
			}
		}
	}

	DynamicsWorld::~DynamicsWorld()
	{
		if(_dynamicsWorld==NULL){
			delete _dynamicsWorld;
		}
		if(_dynamicsWorld==NULL){
			delete _constraintSolver;
		}
		if(_dispatcher==NULL){
			delete _dispatcher;
		}
		if(_collisionConfiguration==NULL){
			delete _collisionConfiguration;
		}
		if(_broadphase==NULL){
			delete _broadphase;
		}
	}

}
